%%%%%三阶卡尔曼滤波的降阶测试实验

%线性规划的二阶卡尔曼滤波实现
%%%%%%该算法使用线性回归预测模型并且融合了两个观测器的数据
%即位置观测器和速度观测器（可以类比陀螺仪与加速度计）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t = 0:0.1:10;  %采样时间
L = length(t); %信号采样为100个
dt = 0.1;   %采样时间间隔
x = zeros(1,L);%初始化状态变量
y = zeros(2,L);%初始化观测变量
%%%%%%%%%%%%%%%%%%%%%%%%%%%生成随机信号：
%%%其中x(i)为对象真实状态
%%%% y(1,i)为位置传感器
%%%%  y(2,i)为速度传感器
for i=1:L
    x(i) = sin(t(i));  %状态变量赋值
    y(1,i) = x(i) + normrnd(0,0.3);  %生成观测数据
    if i>1   %%%%%%生成速度传感器观测数据
        y(2,i)=  (x(i)- x(i-1))/dt + normrnd(0,0.3) ;   
    else
        y(2,1)= normrnd(0,0.2);
    end
end
%%%%%%%%信号生成完毕%%%%%%%%%%%

%  %%%%参数列表初始化%%%  %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
F = [1,  dt ;  0,  1];  %预测矩阵
H = [1,0; 0,1];  %观测矩阵
Q1 = [0.01, 0 ;  0, 0.05];  %预测噪声协方差矩阵
R1 = [2, 0;  0, 0.5];  %观测噪声协方差矩阵
%%%%%%%%%%%   X意为均值   P意为方差
Xplus = zeros(2,L); %初始化 状态更新变量  plus意为“后验”  minus意为“先验”
Xplus(1,1)= 0.1;  %预测状态变量矩阵赋初值（可以随意给）
Xplus(2,1)=0;   %%预测初始速度与加速度均为零
Pplus=[0.2 , 0;  0, 0.2];    %初始化 协方差更新变量
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%参数建立完毕，开始滤波

for i=2:L
    %%%%%预测步%%%%
    Xminus = F*Xplus(:,i-1);
    Pminus = F*Pplus*F' + Q1;
    %%%%更新步%%%%
    K = (Pminus*H')*inv(H*Pminus*H'+R1); %卡尔曼增益系数矩阵
    Xplus(:,i) = Xminus+ K*(y(:,i)-H*Xminus);
    Pplus = (eye(2)- K*H)*Pminus;
end
%%%%%%%%%%%绘图%%%%%%%%%%%%%%%
plot(t,Xplus(1,:),'b',t,y(1,:),'r',t,x,'g','LineWidth',1);
